#ifndef SYSTEM_H
#define SYSTEM_H

#include<thread>
#include<mutex>

#include "Tracking.h"
#include "Localoptim.h"
#include "Settings.h"

#include "nav_msgs/Odometry.h"

namespace msf
{
class Viewer;
class MapDrawer;
class Atlas;
class Tracking;
class LocalOptimizing;
class Settings;

class System
{
public:
//输入传感器
      enum eSensor{
            IMU_ODOM=0
      };

public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
      //初始化定位系统，包括启动定位，优化和可视化线程
      System(const string &strSettingsFile, const eSensor sensor, const bool bUseViewer = true, const int initFr = 0);

      //处理给定的数据帧，返回位姿（跟踪失败为空）
      Sophus::SE3f Track(const nav_msgs::OdometryConstPtr &odom, const double &timestamp, const vector<IMU::Point>& vImuMeas);

      // This stops local optimizing thread (map building) and performs only eskf tracking.
      void ActivateLocalizationMode();
      // This resumes local optimizing thread and performs msf again.
      void DeactivateLocalizationMode();      

      // 重置系统(清除Atlas或当前激活地图)
      void Reset();
      void ResetActiveMap();

      // 所有线程被要求结束。会等待直到所有线程结束。在保存轨迹前一定要被调用。
      void Shutdown();
      bool isShutDown();

      // 保存轨迹
      void SaveTrajectoryEuRoC(const string &filename);
      void SaveKeyPoseFrameTrajectoryEuRoC(const string &filename);

      //可以在Track之后调用
      int GetTrackingState();
    
private:

      // 输入传感器
      eSensor mSensor;

      // 地图结构保存所有关键位姿帧的指针
      Atlas* mpAtlas;

      // 位姿跟踪。接收一个数据帧并计算当前位姿。
      // 也决定何时插入新的keypose
      Tracking* mpTracker;

      // 局部优化器。进行局部轨迹的BA
      LocalOptimizing* mpLocaloptimizer;

      // 绘制轨迹。使用Pangolin
      Viewer* mpViewer;

      MapDrawer* mpMapDrawer;

      // 系统线程：轨迹局部优化和轨迹可视化
      // 跟踪线程在创造System对象的主线程中
      std::thread* mpLocaloptimizing;
      std::thread* mptViewer;

      // 重置flag
      std::mutex mMutexReset;
      bool mbReset;
      bool mbResetActiveMap;
      
      // Change mode flags
      std::mutex mMutexMode;
      bool mbActivateLocalizationMode;
      bool mbDeactivateLocalizationMode;      

      // 关闭flag
      bool mbShutDown;

      // 跟踪状态
      int mTrackingState;
      std::mutex mMutexState;

      Settings* settings_;
};

}// namespace msf

#endif